﻿#include "pipelineunstack.h"
#include "componentmanager.h"
#include "wcs/WCSSend.h"
#include "./unstackplate/unstackplatemanager.h"
#include "boxline/boxline.h"
#include "project_data.h"
#include "commonTools/commonfunctionset.h"
#include <QList>
#include "./SystemConfiguration/systemconfiguration.h"

#include "pipeline_log.h"


PipelineUnstack::PipelineUnstack()
{}

PipelineUnstack::~PipelineUnstack()
{}
bool PipelineUnstack::Init()
{
	if (m_info.type != PipelineType::pipeline_unstack) {
		log_error("PIpeline type is error, type = {}", (int)m_info.type);
		return false;
	}

	m_wcs = dynamic_cast<WCSSend*>(component->GetComponent(Component::component_wcsSend));
	if (m_wcs == nullptr) {
		log_error("Get wcs send comp failed");
		return false;
	}

	auto plateM = component->GetComponent(Component::component_unstackPlate);
	if (plateM == nullptr) {
		log_error("Get stack plateM comp failed");
		return false;
	}
	m_pick = static_cast<UnstackPlate*>(plateM->GetPoint(m_info.grab));
	if (m_pick == nullptr) {
		log_error("Get stack plate [{}] failed", m_info.grab);
		return false;
	}

	auto lineM = component->GetComponent(Component::component_boxline);
	if (lineM == nullptr) {
		log_error("Get BoxlineM comp failed");
		return false;
	}

	m_put = static_cast<Boxline*>(lineM->GetPoint(m_info.putdown));
	if (m_put == nullptr) {
		log_error("Get boxlinde [{}] failed", m_info.putdown);
		return false;
	}

	return true;
}

bool PipelineUnstack::CheckTaskData(const QByteArray& task)
{
	TaskDataJ data = {};
	data.fromJson(task);
	if (data.robNo() != m_info.rob) {
		log_error("任务数据机械手编号不匹配，任务：{}， 本机{}", data.robNo(), m_info.rob);
		return false;
	}
	if (data.skuList().size() != 1) {
		return false;
	}
	if (m_info.putdown != data.putNo() || m_info.grab != data.pickNo()) {
		return false;
	}
	auto& sku = data.skuList().at(0);
	if (sku.height() <= 0 || sku.length() <= 0 || sku.width() <= 0) {
		log_error("Sku Size error");
		return false;
	}
	if (data.taskNum() <= 0) {
		log_error("task Num error");
		return false;
	}

	log_trace("{} reciever a task id = {}", m_info.id, data.taskId());
	return true;
}

int PipelineUnstack::IfTaskAvailable(const QByteArray& task)
{
	TaskDataJ data = {};
	data.fromJson(task);
	if (!CheckTaskData(task)) {
		return -1;
	}
	if (m_task.isBusy) {
		SetTraceMassage(QString("当前正在执行任务 %1，无法接收新任务（%2）").arg(m_task.taskId).arg(data.taskId()));
		log_error("Task matching, but there are unfinished tasks");
		return 1;
	}
	m_task.stackType = 0;
	m_task.taskId = data.taskId();
	m_task.taskNum = data.taskNum();
	auto& sku = data.skuList().at(0);
	m_sku.skuNo = sku.skuNo();
	m_sku.pos = 0;
	m_sku.size.width = sku.width();
	m_sku.size.height = sku.height();
	m_sku.size.length = sku.length();
	m_sku.type = sku.boxType();
	m_sku.weight = sku.weight();
	m_sku.valied = true;
	m_task.isBusy = true;
	m_sku.valied = true;
	SetTraceMassage(QString("收到新任务 %1, 任务数量：%2").arg(m_task.taskId).arg(m_task.taskNum));
	return 0;
}

/****************************************************************************
 * 功能: 任务初始化操作，由项目具体实现
 * 返回值: true：执行成功，业务线切换到任务初始化完成状态
 ****************************************************************************/
bool PipelineUnstack::TaskInit()
{
	m_waitFinishStep.clear();
	m_execStep.clear();
	m_prepareStep.clear();
	m_snapflag.storeRelease(true); // 设置为允许拍照状态
	return true;
}

/****************************************************************************
 * 功能: 判断任务启动条件是否具备
 * 返回值:true：任务可以启动，业务线切换到任务开始状态
 ****************************************************************************/
bool PipelineUnstack::IfTaskStarted()
{
	return true;
}

/****************************************************************************
 * 功能: 判断任务是否准备完成
 * 返回值: true: 任务已经准备完成，直接进入任务等待状态
 ****************************************************************************/
bool PipelineUnstack::IfTaskPrepared()
{
	log_trace("taskNum: {}, finishNum:{}, exec Num:{}", m_task.taskNum, m_exec.finishNum, m_exec.execNum);
	if (m_task.taskNum - m_exec.finishNum - m_exec.execNum <= 0) {
		log_trace("所有节拍生成完成");
		return true;
	}

	return false;
}

int PipelineUnstack::StepPrepare()
{
	if (!m_snapflag.loadAcquire()) {	// 不能触发拍照
		return 1;
	} else {	// 生成新节拍
		if (m_prepareStep.valied) {
			log_error("准备节拍数据不为空，无法生成新数据");
			SetPipelineErrorState(QString("准备节拍数据不为空，无法生成新数据, bug"));
			return -1;
		}
		m_prepareStep.id = m_stepId++;
		m_prepareStep.valied = true;
	}

	int red = m_task.taskNum - m_exec.finishNum - m_exec.execNum;
	if (red <= 0) {
		SetPipelineErrorState("程序bug, 任务剩余数量为0, 但是仍然产生节拍");
		return -1;
	}
	VisualParam param = {};
	param.jig = 1;	// 夹具恒定为1
	param.sku = m_sku.size;
	param.taskNum = red;	// 传入最大数量，支持多抓
	VisualResult pickResult = {};
	bool ret = m_pick->TakeVisualResult(param, pickResult);
	if (!ret) {
		SetPipelineErrorState("拆垛托盘算法计算失败:" + pickResult.msg);
		return -1;
	}
	if (pickResult.num <= 0) {
		SetPipelineErrorState("拆垛托盘算法计算结果异常：" + pickResult.msg);
		return -1;
	}

	m_snapflag.storeRelease(false);
	m_prepareStep.grabPos = pickResult.grabPos;
	m_prepareStep.excuteNum = pickResult.num;
	log_trace("抓取计算结果： 抓取数量：{}，抓取位置（{}，{}，{}，{}，{}，{}）, 夹具位姿 （{}，{}，{}，{}，{}，{}）",
		pickResult.num, pickResult.grabPos.x, pickResult.grabPos.y, pickResult.grabPos.z,
		pickResult.grabPos.a, pickResult.grabPos.b, pickResult.grabPos.c, pickResult.jigInSku.x,
		pickResult.jigInSku.y, pickResult.jigInSku.z, pickResult.jigInSku.a, pickResult.jigInSku.b, pickResult.jigInSku.c);
	SetTraceMassage(QString("节拍%1，抓取数量为%2").arg(m_prepareStep.id).arg(m_prepareStep.excuteNum));
	QString err; 
	m_put->UpdateSkuInfo(m_sku);
	if (!m_put->CalculateNextGrab(m_prepareStep.id, m_prepareStep.excuteNum, err)) {
		SetPipelineErrorState(QString("放货点[%1]计算抓取坐标失败：%2").arg(m_info.putdown).arg(err));
		return -1;
	}
	Position putPos = {};	// 物料的中心点坐标
	if (!m_put->GetSkuPosition(m_prepareStep.id, putPos)) {
		SetPipelineErrorState(QString("放货点[%1]坐标失败").arg(m_info.grab));
		return -1;
	}
	Position robInjig = {};
	QList<JigSetting> jigs = syscfg->GetJigSetting();
	for (auto& iter : jigs) {
		if (iter.id == param.jig) {
			robInjig = iter.rob;
		}
	}
	if (robInjig.c != 180) {
		SetPipelineErrorState(QString("夹具选择出错").arg(param.jig));
		return -1;
	}
	auto temp = CommonFun::TransAToBCoordinateSystem(pickResult.jigInSku, putPos);
	m_prepareStep.putPos = CommonFun::TransAToBCoordinateSystem(robInjig, temp);

	log_trace("放置位置计算结果：物料位置（{}，{}，{}，{}，{}，{}）,  放货位置（{}，{}，{}，{}，{}，{}）",
		putPos.x, putPos.y, putPos.z, putPos.a, putPos.b, putPos.c, m_prepareStep.putPos.x,
		m_prepareStep.putPos.y, m_prepareStep.putPos.z, m_prepareStep.putPos.a, m_prepareStep.putPos.b, m_prepareStep.putPos.c);

	QString path = "Z-0,Z-1,Z-2,Z-3";
	AddPathListToPrepareStepInfo(path);
	SetTraceMassage(QString("节拍%1准备完成，路径：%2").arg(m_prepareStep.id).arg(path));;
	return 0;
}

/****************************************************************************
 * 功能: 任务完成处理操作， 任务完成状态时被调用
 * 返回值: true 表示成功
 ****************************************************************************/
bool PipelineUnstack::TaskFinish()
{
	SetTraceMassage(QString("任务执行完成，发送任务完成通知"));
	TaskFinishInfo data = {};
	data.robNo = m_info.rob;
	data.taskId = m_task.taskId;
	data.finishNum = m_exec.finishNum;
	data.taskFinish = m_exec.finishNum < m_task.taskNum;

	data.plateId = m_info.putdown;
	QString err;
	if (!m_wcs->SendTaskFinishInfo(data.toByteArray(), err)) {
		log_error("send Task Finish info failed, err = {}", err);
		SetErrorMassage(QString("发送任务完成通知失败，%1").arg(err));
		return false;
	}
	DataInit();
	return true;

}

bool PipelineUnstack::TaskContinue()
{
	m_snapflag.storeRelease(true);
	return PipelineBase::TaskContinue();
}

#define SKU_ARRIVE ("/sku/arrived")

bool PipelineUnstack::SendArriveInfo()
{
	SkuArriveInfo info = {};
	info.robNo = m_info.rob;
	info.taskId = m_task.taskId;
	info.putNo = m_info.putdown;
	info.QRInfo = "";
	info.num = m_waitFinishStep.excuteNum;
	info.msgId = m_waitFinishStep.id;
	QByteArray ans = {};
	if (m_wcs->SendToWCS(SKU_ARRIVE, info.toByteArray(), ans)) {
		WCSAns a = {};
		a.fromJson(ans);
		if (a.code() != 0) {
			SetErrorMassage("到位通知发送异常：" + a.msg());
			return false;
		} else {
			SetTraceMassage("到位通知发送完成");
			return true;
		}
	} else {
		SetErrorMassage("到位通知发送失败：" + ans);
		return false;
	}
}


bool PipelineUnstack::SendNotifyer(int type)
{
	if (type == 1) {	// 发送物料到位通知
		return SendArriveInfo();
	}
	log_error("Send notifiy, type = {}", type);
	return false;
}

bool  PipelineUnstack::TakePicture()
{ 
	m_snapflag.storeRelease(true);
	log_trace("{}允许拍照", m_info.id);
	return true;
}
/*路径执行前操作*/
bool PipelineUnstack::PathStart(const QString& path)
{
	QStringList steps = path.split("-");
	if (steps.size() != 2) {
		SetErrorMassage("路径名称解析失败：" + path);
		return false;
	} else {
		if (steps.at(1) == "2") {
			int state = m_put->GetState();
			if (state == 0) {	
				return true;
			} else if (state == 1) {
				SetTraceMassage(QString("放货位置%1禁止放货").arg(m_info.id));
				return false;
			} else {
				SetErrorMassage(QString("放货位置状态%1信息获取异常").arg(m_info.id));
				return false;
			}
		}
	}

	return true;
}


/*路径执行后特殊操作*/
bool PipelineUnstack::PathEnd(const QString& path)
{
	QStringList steps = path.split("-");
	if (steps.size() != 2) {
		SetErrorMassage("路径名称解析失败：" + path);
		return false;
	} else {
		if (steps.at(1) == "3") {
			if (m_put->SetSkuArrived(1)) {
				return false;
			} else {
				return true;
			}
		}
	}
	SetTraceMassage(QString("业务线%1路径%2执行完成").arg(m_info.id).arg(path));
	return true;
}
